//Declare the servo
#include <Servo.h>
#include "WProgram.h"
void setup();
void loop();
void interpretInput();
void step(int num, int motor, int newdirection);
int doStep(int motor);
void openScanner();
void closeScanner();
void toggleVac();
void lowerAndLift(int taken_before);
int averagedInfraredReflectance();
void retractAndTest(int extra);
int determineExtraSteps(double ratio, double num_taken_before);
void moveActuator(int pwm, int seconds, char thedirection);
void zeroYAxis();
void zeroZAxis();
void zeroXAxis();
void zeroTheMachine();
void gotoAbsCoords(int coords[]);
void gotoAbs(int x, int y, int z);
void echoData();
void echoArmSensorDebugInfo();
Servo servo1;

//Set X Axis Pins
int x_dir = 53;
int x_step = 52;
int x_sleep = 51;

//Set Z Axis Pins
int z_dir = 24;
int z_step = 28;
int z_sleep = 37;

//Set Limit Switch Pins
int ZBottomLimit = 38;
int YBackLimit = 45;
int ZTopLimit = 33;
int XLeftLimit = 26;
int XRightLimit = 29;

//Set actuator Pins
int actuator_in1 = 22;
int actuator_in2 = 23;
int actuator_pwm = 7;
int actuator_standby = 25;

//Set vacuum pin
int vacuum = 31;

//Set Analog Sensor Pins
int photoSensor = 4;
int infraredReflectance = 5;

//Set constants
int x_speed = 2000;
int z_speed = 1000;
int centerCoords[] = {15000, 7000, 0};
int bin1Coords[] = {4000, 6000, 0};
int bin2Coords[] = {26000, 7000, 0};
int retreatCoords[] = {5000, 1000, 0};

//Set variables
long x_stepPosition = 0;
long z_stepPosition = 0;
long y_position = 0;
boolean zDirection = HIGH;
boolean xDirection = HIGH;
int vac_state = 0;
int photoSensorValue = 0;
int infraredSensorValue = 0;
int x_sleep_state = 0;
int z_sleep_state = 0;
int errorNum = 0;


void setup() {
  
  //Initialize serial control
  Serial.begin(115200);
  
  //Set switch pins to inputs and turn on the pullup resistors
  pinMode(ZBottomLimit, INPUT);
  digitalWrite(ZBottomLimit, HIGH); // turn on pullup resistors
  pinMode(ZTopLimit, INPUT);
  digitalWrite(ZTopLimit, HIGH); // turn on pullup resistors
  pinMode(YBackLimit, INPUT);
  digitalWrite(YBackLimit, HIGH); // turn on pullup resistors
  pinMode(XRightLimit, INPUT);
  digitalWrite(XRightLimit, HIGH); // turn on pullup resistors
  pinMode(XLeftLimit, INPUT);
  digitalWrite(XLeftLimit, HIGH); // turn on pullup resistors
  
  //Declare the vacuum pin
  pinMode(vacuum, OUTPUT);
  
  //Connect up the servo
  servo1.attach(10);
  
  //Set initial states for actuator pins
  pinMode(actuator_in1, OUTPUT);
  pinMode(actuator_in2, OUTPUT);
  pinMode(actuator_standby, OUTPUT);
  digitalWrite(actuator_standby, HIGH); 
  digitalWrite(actuator_in1,HIGH);
  digitalWrite(actuator_in2,HIGH); 
  
  //Set initial states for X axis movement pins
  pinMode(x_dir, OUTPUT);
  pinMode(x_step, OUTPUT);
  pinMode(x_sleep, OUTPUT);
  digitalWrite(x_sleep, LOW);
  digitalWrite(x_step, LOW);
  
  //Set initial states for Z axis movement pins
  pinMode(z_dir, OUTPUT);
  pinMode(z_step, OUTPUT);
  pinMode(z_sleep, OUTPUT);
  digitalWrite(z_sleep, LOW);
  digitalWrite(z_step, LOW);  
  
  //Move the servo to the full open position
  servo1.write(180);

}

void loop() {
  interpretInput();
}

void interpretInput() {
    //Interpret input
  static int v = 0;
  if ( Serial.available()) {
    char ch = Serial.read();
    switch(ch) {
      case '0'...'9':
        v = v * 10 + ch - '0';
        break;
      case 'x':
        step(v, 1, xDirection);
        v = 0;
        break;
      case 'z':
        step(v, 0, zDirection);
        v = 0;
        break;
      case 'p':
        digitalWrite(x_sleep, HIGH);
        break;
      case 'f':
        xDirection = LOW;
        break;
      case 'b':
        xDirection = HIGH;
        break;
      case 'u':
        zDirection = LOW;
        break;
      case 'd':
        zDirection = HIGH;
        break;
      case 'q':
        moveActuator(200,1000, 'b');
        break;
      case 'w':
        moveActuator(200,1000,'f');
        break;
      case 'v':
        toggleVac();
        break;
      case 'o':
        openScanner();
        break;
      case 'c':
        closeScanner();
        break;
      case 'l':
        lowerAndLift(0);
        break;
      case 'i':
        echoArmSensorDebugInfo();
        break;
      case 'D':
        echoData();
        break;
      case 'y':
        zeroYAxis();
        break;
      case 'Z':
        zeroZAxis();
        break;
      case 'X':
        zeroXAxis();
        break;
      case 'M':
        zeroTheMachine();
        break;
      case 'g':
        gotoAbsCoords(centerCoords);
        break;
      case 'h':
        gotoAbsCoords(bin1Coords);
        break;
      case 'j':
        gotoAbsCoords(retreatCoords);
        break;
      case 'k':
        gotoAbsCoords(bin2Coords);
        break;
    }
  }
  
}

//Moves the stepper motors on the X and Z Axes
//Motor 0 = Z, Motor 1 = X
void step(int num, int motor, int newdirection) {
  int currentSpeed = 0;
  if (motor == 1) {
      digitalWrite(x_dir, newdirection);
      currentSpeed = x_speed;
  } else {
      digitalWrite(z_dir, newdirection);
      currentSpeed = z_speed;
  }
  int i = 0;
  while(i < num) {
    doStep(motor);
    if(motor == 1 && newdirection == LOW) {
      x_stepPosition += 1;
    } else if (motor == 1 && newdirection == HIGH) {
      x_stepPosition -= 1;
    } else if (motor == 0 && newdirection == HIGH) {
      z_stepPosition += 1;
    } else if (motor == 0 && newdirection == LOW) {
      z_stepPosition -= 1;
    }
    delayMicroseconds(int(currentSpeed));
    i++;
  }
}

//Performs an actual step command
int doStep(int motor) {
  if (motor == 1) {
      digitalWrite(x_step, HIGH);
      digitalWrite(x_step,LOW);
  } else {
      digitalWrite(z_step, HIGH);
      digitalWrite(z_step,LOW);
  }
}

//Opens the scanner lid
void openScanner() {
  int pos;
  for(pos = 180; pos>=0; pos-=1)     // sweep from 120 to 0 degrees in steps of 1
 {                                
   servo1.write(pos);           // sweep end position 
   delay(15);                            // 15ms delay for sweep
 } 
}

//Closes the scanner lid
void closeScanner() {
  int pos;
  for(pos = 0; pos < 180; pos +=1)   // sweep from 0 to 120 degrees in steps of 1
 {
   servo1.write(pos);           // sweep start position 
   delay(15);                            // 15ms delay for sweep
 } 
}

//Toggles the vacuum on and off
void toggleVac() {
      if (vac_state == 0) {
            digitalWrite(vacuum, HIGH); 
            vac_state = 1; 
        } else if (vac_state == 1) {
           digitalWrite(vacuum, LOW); 
           vac_state = 0;
        }
}

//Drops the Z axis to pick up an item
void lowerAndLift(int taken_before) {
  int j = analogRead(photoSensor);
  double i = double(analogRead(photoSensor)) / double(j);
  while (i > .4) {
      step(100, 0, HIGH);
      i = double(analogRead(photoSensor)) / double(j);
      Serial.println(i);
  }
  Serial.println("At level of object");
  echoArmSensorDebugInfo();
  int extra = determineExtraSteps(double(analogRead(photoSensor)) / double(analogRead(infraredReflectance)), taken_before);
  step(extra, 0, HIGH);
  if (taken_before == 0) {
  toggleVac();
  delay(1000);
  }
  retractAndTest(extra);
  //echoArmSensorDebugInfo();
}

int averagedInfraredReflectance() {
  int initialInfrared;
  for(int i = 0; i < 5; i++) {
    initialInfrared += analogRead(infraredReflectance);
    delay(10);
  } 
  return initialInfrared / 5;
}
void retractAndTest(int extra) {
  int initialInfrared = averagedInfraredReflectance();
  step(2000 + extra, 0, LOW);
  Serial.print("Difference in infrared");
  Serial.println(averagedInfraredReflectance() - initialInfrared);
  if(abs(averagedInfraredReflectance() - initialInfrared) > 50) {
    lowerAndLift(extra);
  } else {
    Serial.println("Got it!");
  }
}

int determineExtraSteps(double ratio, double num_taken_before) {
  if (num_taken_before == 0) {
    return 1000;
  } else {
    Serial.println("Trying more steps");
    return num_taken_before + 500;
  }
}

void moveActuator(int pwm, int seconds, char thedirection) {
        digitalWrite(actuator_standby, HIGH); 
        if (thedirection == 'f') {
            digitalWrite(actuator_in1,HIGH);
            digitalWrite(actuator_in2,LOW); 
            y_position += seconds;
        } else if (thedirection == 'b') {
            digitalWrite(actuator_in1,LOW);
            digitalWrite(actuator_in2,HIGH); 
            y_position -= seconds;
        }
        analogWrite(actuator_pwm, pwm);
        delay(seconds);
        digitalWrite(actuator_in1,HIGH);
        digitalWrite(actuator_in2,HIGH);
        digitalWrite(actuator_standby, LOW);
        
}

void zeroYAxis() {
  boolean triggered = digitalRead(YBackLimit);
  while (triggered == HIGH) {
    moveActuator(200, 200, 'b');
    triggered = digitalRead(YBackLimit);
  }
  //Serial.println("Zeroed");
  y_position = 0;
  
}

void zeroZAxis() {
  boolean triggered = digitalRead(ZTopLimit);
  while (triggered == HIGH) {
    step(200, 0, LOW);
    triggered = digitalRead(ZTopLimit);
  }
  //Serial.println("Zeroed");
  z_stepPosition = 0;
}

void zeroXAxis() {
  boolean triggered = digitalRead(XLeftLimit);
  while (triggered == HIGH) {
    step(50, 1, HIGH);
    triggered = digitalRead(XLeftLimit);
  }
  //Serial.println("Zeroed");
  x_stepPosition = 0;
}

void zeroTheMachine() {
  zeroZAxis();
  zeroXAxis();
  zeroYAxis();
}
void gotoAbsCoords(int coords[]) {
  int x = coords[0];
  int y = coords[1];
  int z = coords[2];
  gotoAbs(x, y, z);
  
}
void gotoAbs(int x, int y, int z) {
  //Move Z Axis
  //If desired position is up
  if(z < z_stepPosition) {
    while (z < z_stepPosition) {
      step(100, 0, LOW);
    }
  } else if (z > z_stepPosition) {
    while (z > z_stepPosition) {
      step(100, 0, HIGH);
    }
  }
  //Move X axis
    if(x < x_stepPosition) {
    while (x < x_stepPosition) {
      step(10, 1, HIGH);
    }
  } else if (x > x_stepPosition) {
    while (x > x_stepPosition) {
      step(10, 1, LOW);
      //Serial.println(x_stepPosition);
    }
  }
  //Move Y axis
    if(y < y_position) {
    while (y < y_position) {
      moveActuator(200, 100, 'b');
    }
  } else if (y > y_position) {
    while (y > y_position) {
      moveActuator(200, 100, 'f');
    }
  }  
}
//Echo data

void echoData() {
  Serial.print("<response>");
  Serial.print("<error>"); Serial.print(errorNum); Serial.print("</error>");
  Serial.print("<z_position>"); Serial.print(z_stepPosition); Serial.print("</z_position>");
  Serial.print("<x_position>"); Serial.print(x_stepPosition); Serial.print("</x_position>");
    Serial.print("<y_position>"); Serial.print(y_position); Serial.print("</y_position>");
  Serial.print("</response>");
  Serial.println();
}

void echoArmSensorDebugInfo() {
          Serial.print("Ratio of photo to infrared ");
        infraredSensorValue = analogRead(infraredReflectance);
        delay(10);
        photoSensorValue = analogRead(photoSensor);
        Serial.print("Photo: ");
        Serial.print(photoSensorValue);
        Serial.print(" Infrared: ");
        Serial.print(infraredSensorValue);
        Serial.print(" Ratio: ");
        double ratio = double(photoSensorValue) / double(infraredSensorValue);
        Serial.print(ratio);
        Serial.println();
}



int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}

